# -*- coding: utf-8 -*-
"""
Created on Mon Mar 28 20:37:06 2022

@author: lizhuang
"""
import numpy as np
def R6ki(x):
    #function y = R6ki(x) 
    # 
    # Position so1ution of a RRR bar group 
    # 
    # so1ving acce1eration of 6R three degree bar group 
    # 
    # C.Opyright 2002 Qu Xiuquan 
    # 
    # Input parameters 
    # 
    #x(1)=ri 
    # x(2) = rj 
    # x(3) = rk 
    # x(4) = e 
    # x(5) = f 
    # x(6) = g 
    # x(7) = theta - i 
    # x(8) = theta
    # x(9) = theta - k 
    # x(1O) = theta
    # x(11) = theta - f 
    # x(12) = theta - g 
    # x(13) = dtheta - i 
    # x(14) = dtheta - j 
    # x(15) = dtheta - k 
    # x(16) = dtheta - e 
    # x(17) = dthe -f
    # x(18) = dtheta - g 
    # x(19) = Re[ddB] 
    # x(20) = Im[ddB] 
    # x(21) = Re[ddC] 
    # x(22) = Im[ddC] 
    # x(23) = Re[ddD] 
    # x(24) = Im[ddD) 
    # 
    # Output parameters
    # 
    # y(1) = ddtheta
    # y(2) = ddtheta - j 
    # y(3) = ddtheta - k 
    # y(4) = ddtheta
    # y(5) = ddtheta - f 
    # y(6) = ddtheta - g 
    # y(7) = Re[ddE] 
    # y(8) = Im[ddE] 
    # y(9) = Re[ddF] 
    # y(IO) = Im[ddF] 
    # y(11) = Re[ddG] 
    # y(12) = Im[ddG) 
    # 
    pi=np.pi;cos=np.cos;sin=np.sin
    a =np.array([[-x[0]*cos(x[6]+pi/2),x[1]*cos(x[7]+pi/2),0 ,0 ,0 ,-x[5]*cos(x[11]+ pi/2)], 
                 [-x[0]*sin(x[6]+pi/2), x[1]*sin(x[7]+pi/2) ,0 ,0 ,0 ,-x[5]*sin(x[11]+pi/2)],
                 [0,-x[1]*cos(x[7]+pi/2),x[2]*cos(x[8]+pi/2),-x[3]*cos(x[9]+pi/2),0,0],
                 [0,-x[1]*sin(x[7]+pi/2),x[2]*sin(x[8]+pi/2),-x[3]*sin(x[9]+pi/2),0,0],
                 [x[0]*cos(x[6]+pi/2),0,-x[2]*cos(x[8]+pi/2),0,-x[4]*cos(x[10]+pi/2),0],
                 [x[0]*sin(x[6]+pi/2),0,-x[2]*sin(x[8]+pi/2),0,-x[4]*sin(x[10]+pi/2),0]])
    print(a)
    c = - np.array([[- x[0] * cos(x[6] + pi) ,x[1] * cos(x[7] + pi) ,0 ,0 ,0 ,- x[5] * cos(x[11] +pi)],
    [- x[0] * sin(x[6] + pi) ,x[1] * sin(x[7] + pi), 0 ,0, 0 ,- x[5] * sin(x[11] +pi)],
    [0 ,- x[1] * cos(x[7] + pi), x[2] * cos(x[8] + pi) ,- x[3] * cos(x[9] + pi), 0 ,0],
    [0 ,- x[1] * sin(x[7] + pi) ,x[2] * sin(x[8] + pi) ,- x[3] * sin(x[9] + pi),0 ,0],
    [x[0] * cos(x[6] + pi) ,0 ,- x[2] * cos(x[8] + pi), 0 ,- x[4] * cos(x[10] + pi), 0],
    [x[0] * sin(x[6] + pi), 0, - x[2] * sin(x[8] + pi), 0, - x[4] * sin(x[10] + pi), 0]])
    b1 = c.dot(np.array([[x[12]**2],
                        [x[13]**2],
                        [x[14]**2],
                        [x[15]**2],
                        [x[16]**2],
                        [x[17]**2]]))
    print(b1)
    b2 = np.array([[x[18]-x[20]],
                   [x[19]-x[21]],
                   [x[20]-x[22]],
                   [x[21]-x[23]],
                   [x[22]-x[18]],
                   [x[23]-x[19]]]) 
    print(b2)
    b=b1+b2; 
    ddtheta=np.linalg.inv(a).dot(b); 
    print(ddtheta)
    y=np.array([0,0,0,0,0,0,0,0,0,0,0,0])
    y[0] = ddtheta[0];
    y[1] = ddtheta[1]; 
    y[2] = ddtheta[2]; 
    y[3] = ddtheta[3]; 
    y[4] = ddtheta[4]; 
    y[5] = ddtheta[5]; 
    y[6] = x[18] + x[0] * ddtheta[0] * cos(x[6] + pi/2) + x[0] * x[12]**2 * cos(x[6] + pi); 
    y[7] = x[19] + x[0] * ddtheta[0] * sin(x[6] + pi/2) + x[0] * x[12]**2* sin(x[6] + pi); 
    y[8] = x[20] + x[1] * ddtheta[1] * cos(x[7] + pi/2) + x[1] * x[13]**2 * cos(x[7] +pi); 
    y[9]= x[21] + x[1] * ddtheta[1] * sin(x[7] + pi/2) + x[1] * x[13]**2 * sin(x[7] +pi); 
    y[10] = x[22] + x[2] * ddtheta[2] * cos(x[8] + pi/2) + x[2] * x[14]**2* cos(x[8] + pi); 
    y[11] = x[23] + x[2] * ddtheta[2] * sin(x[8] + pi/2) + x[2] * x[14]**2 * sin(x[8] +pi);
    return y
x=np.array([1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,12,22,23,24])
y=R6ki(x)